跳到主要内容

Mahony 非线性互补滤波器

1. 数学推导​

本节按一条主线展开:先建立陀螺 / 加速度计 / 磁力计的误差模型,再说明如何用方向观测约束姿态(Wahba),接着给出姿态运动学与误差代价,最后合拢为 Mahony 的显式互补滤波器(ECF)。代码实现见第 2 节,各步与公式的对应关系集中在那里说明。

1.1 陀螺仪模型​

MEMS 陀螺仪利用科里奥利效应测量角速度:内部以固定频率振动的质量块在载体转动时受到与角速度成正比的科氏力,从而输出与机体坐标系下真实角速度 Ω\Omega 成正比的电信号。

实际输出还受到两类误差污染:

  1. 确定性误差(可估计):制造缺陷、温度漂移、电路失调等,使零输入时出现非零、缓慢变化的输出。短时间内近似为常数,通常建模为零点偏置 bb。工程上温漂往往占大头。
  2. 随机误差(不可预测):热噪声、量化噪声等,建模为零均值随机过程 μ\mu(常取高斯白噪声)。

三者线性叠加,得到陀螺仪的加性误差模型:

Ωy=Ω+b+μ∈R3(1.1)\Omega_y = \Omega + b + \mu \in \mathbb{R}^3 \tag{1.1}

其中 Ωy\Omega_y 为测量值,Ω\Omega 为真值,bb 为待在线估计的慢变偏置,μ\mu 为只能滤波抑制的噪声。

建模思路

“真值 + 慢变偏置 + 白噪声”按时间尺度分离误差:慢变量 bb 交给滤波器积分项 kIk_I 估计并补偿;高频噪声 μ\mu 交给低通 / 融合结构衰减。加速度计模型同构。

1.2 加速度计模型​

MEMS 加速度计是“弹簧–质量块–阻尼”结构:壳体加速度使质量块相对壳体位移,电路读出该位移。关键点:自由落体时相对位移为零、输出为零——加速度计测的不是坐标加速度,而是比力(Specific Force):单位质量所受、除重力以外的合外力。

设载体在世界系(W)下速度为 vv,坐标加速度为 v˙\dot v。牛顿第二定律给出:

mv˙=f+mg0⇒f/m=v˙−g0(1.2)m \dot v = f + m g_0 \quad\Rightarrow\quad f / m = \dot v - g_0 \tag{1.2}

加速度计感知的正是 f/mf/m。传感器固连于机体,输出为机体系(B)分量。记 R∈SO(3)R \in SO(3) 满足 vW=R vBv^W = R\, v^B,则 RTR^T 完成 W→B 变换。叠加快变偏置与噪声后:

a=RT(v˙−g0)+ba+μa∈R3(1.3)a = R^T (\dot v - g_0) + b_a + \mu_a \in \mathbb{R}^3 \tag{1.3}

其中 aa 为机体系比力测量,g0=[0,0,−g]Tg_0 = [0, 0, -g]^T 为世界系重力(z 轴向上),bab_a、μa\mu_a 分别为偏置与噪声。

准静态下充当“重力方向传感器”

当 v˙≈0\dot v \approx 0(静止或匀速)且忽略偏置与噪声时,

a≈−RTg0(1.4)a \approx -R^T g_0 \tag{1.4}

即读数给出重力在机体系下的投影方向。世界系重力方向 g0g_0 已知,故可约束姿态中的俯仰与横滚;航向(Yaw)绕重力轴不可观,因此纯 6 轴 IMU 只能估两轴,需磁力计补齐 Yaw。

准静态假设失效

急加速、震动、离心等使 v˙\dot v 不可忽略时,加速度计读数掺入非引力加速度,仍当“重力方向”用会引入姿态误差。这也解释了比例增益 kPk_P 不宜过大:它反映对“当前读数是否真在测重力”的信任度。

1.3 观测方程的规范化​

姿态融合只用方向信息,量纲与模长不重要。世界系 ∣g0∣≈9.8 m/s2|g_0| \approx 9.8\ \mathrm{m/s}^2,将加速度计输出单位化:

va=a/∣a∣≈−RTe3(1.5)v_a = a / |a| \approx -R^T e_3 \tag{1.5}

其中 e3=g0/∣g0∣=[0,0,1]Te_3 = g_0 / |g_0| = [0, 0, 1]^T。式 (1.5) 表明:归一化测量 vav_a 等于世界系 z 轴在机体系下的投影(差一个负号),亦即 RTR^T 的第三列方向。

给定当前姿态估计(四元数 qq),可预测“若 qq 正确,加速度计应读到的单位方向”v^a=R(q)Te3\hat v_a = R(q)^T e_3,分量即旋转矩阵第三列:

v^a=[2(qxqz−qwqy)2(qwqx+qyqz)qw2−qx2−qy2+qz2](1.6)\hat v_a = \begin{bmatrix} 2(q_x q_z - q_w q_y) \\ 2(q_w q_x + q_y q_z) \\ q_w^2 - q_x^2 - q_y^2 + q_z^2 \end{bmatrix} \tag{1.6}

测量方向 vav_a 与预测方向 v^a\hat v_a 均为单位向量,夹角即姿态误差的几何量度。取叉乘:

e=va×v^a(1.7)e = v_a \times \hat v_a \tag{1.7}

ee 的模长正比于夹角正弦(小角度下约等于夹角本身),方向沿“把 v^a\hat v_a 旋到 vav_a”的旋转轴。Mahony 用这一几何量构造反馈,而不是每步显式解姿态方程。

1.4 磁力计模型(9 轴补充观测)​

重力只能约束 Roll / Pitch。要恢复 Yaw,需要世界系中另一已知、且不与重力平行的参考矢量——地磁场最常用。磁力计同样服从加性模型:

m=RTh+Bm+μb∈R3(1.8)m = R^T h + B_m + \mu_b \in \mathbb{R}^3 \tag{1.8}

其中 hh 为世界系地磁矢量(可由 WMM/IGRF 给出),BmB_m 为本地磁扰动,μb\mu_b 为噪声。只关心方向时单位化:

vm=m/∣m∣(1.9)v_m = m / |m| \tag{1.9}
磁力计的两个坑
  1. 本地磁扰动 BmB_m:室内金属、电机电流等可使 BmB_m 显著甚至压过 hh。使用前宜做硬磁 / 软磁标定,运行时监控 ∣m∣|m| 是否偏离正常地磁强度(中国大陆约 50 μT50\ \mu\mathrm{T})。
  2. 磁倾角:hh 多数纬度并不水平。磁力计给出的是地磁矢量方向,解航向前通常先用重力把磁矢量投影到水平面。

1.5 Wahba 问题:从多方向观测恢复姿态​

给定若干“参考方向 ↔ 测量方向”配对,求最优旋转,使参考矢量经旋转后与测量尽可能重合——这就是 Wahba 问题(Wahba, 1965)。标准表述为:寻找 T∈SO(3)T \in SO(3),最小化

J(T)=12∑i=1nwi ∥bi−T ai∥2(1.10)J(T) = \frac{1}{2} \sum_{i=1}^{n} w_i \, \lVert b_i - T\, a_i \rVert^2 \tag{1.10}
  • aia_i:参考系(惯性 / 世界系)单位方向;
  • bib_i:本体坐标系测量单位向量;
  • wi>0w_i > 0:传感器权重;
  • T∈SO(3)T \in SO(3):TTT=IT^T T = I 且 det⁡T=+1\det T = +1。

最优姿态为

T∗=arg⁡min⁡T∈SO(3)J(T)(1.11)T^\ast = \arg\min_{T \in SO(3)} J(T) \tag{1.11}
为何强调 T∈SO(3)T \in SO(3)

若放开约束,J(T)J(T) 的最小二乘解可能含缩放或反射,不再是合法姿态。SO(3)SO(3) 约束正是 Wahba 相对一般最小二乘的难点所在。

航天领域经典解法包括 TRIAD(n=2n=2,快)、Davenport q-method、QUEST、SVD 法等,将在最优化相关栏目介绍。对 9 轴 IMU,n=2n=2:

  • 重力:a1=e3a_1 = e_3,b1=−vab_1 = -v_a,权重 λ1\lambda_1;
  • 地磁:a2=vm∗a_2 = v_m^\ast,b2=vmb_2 = v_m,权重 λ2\lambda_2。

代入得

Ty=arg⁡min⁡T∈SO(3)[λ1 ∥e3−T (−va)∥2+λ2 ∥vm∗−T vm∥2](1.12)T_y = \arg\min_{T \in SO(3)} \Bigl[ \lambda_1\, \lVert e_3 - T\, (-v_a) \rVert^2 + \lambda_2\, \lVert v_m^\ast - T\, v_m \rVert^2 \Bigr] \tag{1.12}

λ1,λ2\lambda_1, \lambda_2 反映对观测的信任度(震动大则减小 λ1\lambda_1,磁扰强则减小 λ2\lambda_2)。几何上:加速度计消掉倾斜两自由度(Tilt = Roll + Pitch),磁力计消掉航向一自由度(Heading = Yaw),恰凑齐 SO(3)SO(3) 的三维。

Mahony 为何不直接解 Wahba

TRIAD / QUEST / SVD 每步开销大,且只用当前帧观测、丢弃陀螺短时高精度信息。Mahony 的做法是:不显式求 T∗T^\ast,而以 T∗T^\ast 与陀螺积分姿态之差为反馈——用式 (1.7) 及磁力计同类叉乘项修正陀螺角速度,使积分过程中姿态向观测收敛。高频信陀螺、低频信加速度计 / 磁力计,即互补滤波之名的由来。

1.6 姿态运动学​

前面讨论的是瞬时几何关系;滤波器处理的是时间序列。问题:已知机体系角速度 Ω(t)\Omega(t),姿态 R(t)R(t) 如何演化?在 SO(3)SO(3) 上:

R˙=R ⌊Ω⌋×(1.13)\dot R = R \, \lfloor \Omega \rfloor_\times \tag{1.13}

其中反对称化算子将 Ω=(Ωx,Ωy,Ωz)T\Omega = (\Omega_x, \Omega_y, \Omega_z)^T 映为

⌊Ω⌋×=[0−ΩzΩyΩz0−Ωx−ΩyΩx0](1.14)\lfloor \Omega \rfloor_\times = \begin{bmatrix} 0 & -\Omega_z & \Omega_y \\ \Omega_z & 0 & -\Omega_x \\ -\Omega_y & \Omega_x & 0 \end{bmatrix} \tag{1.14}

逆运算记为 Ω=vex(⌊Ω⌋×)\Omega = \mathrm{vex}(\lfloor \Omega \rfloor_\times)。对任意向量 uu 有

⌊Ω⌋×u=Ω×u(1.15)\lfloor \Omega \rfloor_\times u = \Omega \times u \tag{1.15}

即反对称矩阵乘向量与叉乘等价——这是 so(3)\mathfrak{so}(3) 与 R3\mathbb{R}^3 的同构。

直接数值积分式 (1.13) 有两处不便:RR 有 9 个元素却只有 3 个自由度;积分会破坏正交性,需反复正交化。工程上多用单位四元数 qq,运动学为

q˙=12 q⊗p(Ω)(1.16)\dot q = \tfrac{1}{2} \, q \otimes p(\Omega) \tag{1.16}

其中 ⊗\otimes 为 Hamilton 乘法,p(Ω)=(0,Ωx,Ωy,Ωz)p(\Omega) = (0, \Omega_x, \Omega_y, \Omega_z)。若 q=(qw,qx,qy,qz)q = (q_w, q_x, q_y, q_z),分量形式为

q˙w=12(−qxΩx−qyΩy−qzΩz)q˙x=12(qwΩx+qyΩz−qzΩy)q˙y=12(qwΩy−qxΩz+qzΩx)q˙z=12(qwΩz+qxΩy−qyΩx)(1.17)\begin{aligned} \dot q_w &= \tfrac{1}{2} ( -q_x \Omega_x - q_y \Omega_y - q_z \Omega_z ) \\ \dot q_x &= \tfrac{1}{2} ( q_w \Omega_x + q_y \Omega_z - q_z \Omega_y ) \\ \dot q_y &= \tfrac{1}{2} ( q_w \Omega_y - q_x \Omega_z + q_z \Omega_x ) \\ \dot q_z &= \tfrac{1}{2} ( q_w \Omega_z + q_x \Omega_y - q_y \Omega_x ) \end{aligned} \tag{1.17}

式 (1.17) 由式 (1.13) 严格导出,是第 2 节四元数更新的出处。

1.7 姿态估计误差与代价函数​

设当前估计为 R^(t)\hat R(t),相对真实姿态 R(t)R(t) 的相对旋转误差定义为

R~:=R^TR(1.18)\tilde R := \hat R^T R \tag{1.18}

估计完美时 R~=I3\tilde R = I_3。用迹代价量化“偏离单位阵”的程度:

Etr=12 tr(I3−R~)(1.19)E_{tr} = \tfrac{1}{2} \, \mathrm{tr}(I_3 - \tilde R) \tag{1.19}

性质:Etr≥0E_{tr} \ge 0(因 tr(R~)≤3\mathrm{tr}(\tilde R) \le 3);Etr=0E_{tr} = 0 当且仅当 R~=I3\tilde R = I_3;若误差四元数为 q~=(q~w,q~v)\tilde q = (\tilde q_w, \tilde q_v),则 Etr=2∣q~v∣2=2(1−q~w2)E_{tr} = 2|\tilde q_v|^2 = 2(1 - \tilde q_w^2)。

滤波器目标:设计 R^(t)\hat R(t) 的动力学,使 EtrE_{tr} 沿轨迹单调降至零。在陀螺积分上加修正项 ω\omega,SO(3) 上互补滤波器的一般形式为

ddtR^=⌊R Ω+kP R^ ω⌋× R^(1.20)\tfrac{d}{dt}\hat R = \big\lfloor R\, \Omega + k_P\, \hat R\, \omega \big\rfloor_\times \, \hat R \tag{1.20}

kP>0k_P > 0 为比例增益。若 ω=0\omega = 0,则 R~\tilde R 不变——纯陀螺积分无法纠正式 (1.1) 中的偏置与噪声带来的漂移。因此 ω\omega 必须来自加速度计 / 磁力计观测。

1.8 显式互补滤波器(Explicit Complementary Filter)​

至此各块可合拢:

环节作用
式 (1.1)陀螺提供短时精准角速度
式 (1.5)–(1.7)加速度计提供长时可信重力方向及叉乘误差
式 (1.8)–(1.9)磁力计提供长时可信地磁方向
式 (1.10)–(1.12)多方向观测可唯一确定姿态(Wahba)
式 (1.13)–(1.17)、(1.18)–(1.20)运动学与误差代价给出滤波器骨架

Mahony 的核心是:如何取修正信号 ω\omega,使代价下降?答案是对各路测量 / 预测方向的加权叉乘求和:

ωmes=∑i=1nki vi×v^i(1.21)\omega_{mes} = \sum_{i=1}^{n} k_i \, v_i \times \hat v_i \tag{1.21}

其中 viv_i 为第 ii 路归一化测量,v^i\hat v_i 为由当前姿态预测的同路方向,ki>0k_i > 0 为信任权重。对应的测量代价可写为

Emes=k1(1−⟨v^a,va⟩)+k2(1−⟨v^m,vm⟩)(1.22)E_{mes} = k_1 (1 - \langle \hat v_a, v_a \rangle) + k_2 (1 - \langle \hat v_m, v_m \rangle) \tag{1.22}

单位向量内积为夹角余弦;完全对齐时代价为 0。Mahony 用 Lyapunov 方法证明:取式 (1.21) 的 ωmes\omega_{mes},EmesE_{mes} 沿轨迹单调下降。几何直觉与式 (1.7) 一致:叉乘指向纠偏旋转轴,叠到陀螺读数上即把预测方向扳回观测方向。

再加上陀螺零偏在线估计,即得显式互补滤波器(ECF):

ddtq^=12 q^⊗p ⁣(Ωy−b^+kP ωmes)(1.23)\tfrac{d}{dt}\hat q = \tfrac{1}{2} \, \hat q \otimes p\!\left( \Omega_y - \hat b + k_P\, \omega_{mes} \right) \tag{1.23}ddtb^=− kI ωmes(1.24)\tfrac{d}{dt}\hat b = -\, k_I \, \omega_{mes} \tag{1.24}ωmes=∑i=1nki vi×v^i(1.25)\omega_{mes} = \sum_{i=1}^n k_i \, v_i \times \hat v_i \tag{1.25}

三条式子的含义:

  1. 式 (1.23):陀螺读数减去偏置估计 b^\hat b,再加上 kPωmesk_P \omega_{mes},得到补偿后角速度再积分。
  2. 式 (1.24):若 ωmes\omega_{mes} 长时间偏向某方向,说明存在系统性零偏,b^\hat b 沿该方向缓慢累加(积分增益 kIk_I)。
  3. 式 (1.25):修正量由各路叉乘加权合成(6 轴 IMU 仅用加速度计一项)。

离散化用前向欧拉:

qt=qt−1+q˙t Δt(1.26)q_t = q_{t-1} + \dot q_t \, \Delta t \tag{1.26}

再对 qtq_t 归一化以保持单位模长。

为何叫“互补”

陀螺高频准、低频漂;加速度计 / 磁力计低频稳、高频易受震动 / 磁扰。滤波器在高频段主要依赖陀螺(kPωmesk_P \omega_{mes} 相对陀螺读数为小量),在低频段用观测拉回漂移——频域上互相补足短板。

增益默认值

原论文常用 kP=1.0k_P = 1.0、kI=0.3k_I = 0.3;下文代码取 Kp=2.0K_p = 2.0、Ki=0.005K_i = 0.005,属工程上按噪声另调的值。原则:kPk_P 大则收敛快但对震动敏感;kIk_I 大则偏置估计快但可能低频振荡。多数场景 kIk_I 取很小正数,只用来慢慢磨掉陀螺偏置。

2. 代码实现​

下面给出 6 轴(陀螺 + 加速度计)实现。各步与第 1 节公式对应如下:

步骤做什么对应公式
第一步归一化加速度计式 (1.5)
第二步由四元数预测重力方向式 (1.6)
第三步叉乘构造误差式 (1.7)、(1.21)
第四步PI:比例项 + 偏置积分式 (1.23)、(1.24)
第五步四元数微分方程式 (1.17)
第六步欧拉积分并归一化式 (1.26)
import numpy as np

# 1. 核心调参参数
Kp = 2.0 # 比例增益:控制加速度计纠正陀螺仪漂移的速度(越大收敛越快,但怕震动)
Ki = 0.005 # 积分增益:消除陀螺仪的固有零点偏置(常置一个极小的值,不需要时可设为 0)

# 2. 内部全局变量(用于积分项累加误差)
e_integral_x = 0.0
e_integral_y = 0.0
e_integral_z = 0.0

def mahony_update_6dof(q, gx, gy, gz, ax, ay, az, dt):
"""
q: 当前姿态四元数 [qw, qx, qy, qz]
gx, gy, gz: 陀螺仪角速度 (单位: rad/s)
ax, ay, az: 加速度计数据 (任意单位,只要是三轴即可)
dt: 采样时间间隔 (单位: 秒)
"""
global e_integral_x, e_integral_y, e_integral_z
qw, qx, qy, qz = q

# 【第一步】归一化加速度计数据 → 式 (1.5)
norm = np.sqrt(ax**2 + ay**2 + az**2)
if norm == 0.0: return q # 处理异常
ax, ay, az = ax / norm, ay / norm, az / norm

# 【第二步】当前四元数下的预测重力方向 → 式 (1.6)
# 旋转矩阵第三列:世界系 [0,0,1] 转到机体系
vx = 2.0 * (qx * qz - qw * qy)
vy = 2.0 * (qw * qx + qy * qz)
vz = qw**2 - qx**2 - qy**2 + qz**2

# 【第三步】测量重力 × 预测重力 → 式 (1.7)
ex = (ay * vz - az * vy)
ey = (az * vx - ax * vz)
ez = (ax * vy - ay * vx)

# 【第四步】PI:比例纠偏 + 偏置积分 → 式 (1.23)、(1.24)
if Ki > 0.0:
e_integral_x += ex * dt
e_integral_y += ey * dt
e_integral_z += ez * dt
else:
e_integral_x, e_integral_y, e_integral_z = 0.0, 0.0, 0.0

gx += Kp * ex + Ki * e_integral_x
gy += Kp * ey + Ki * e_integral_y
gz += Kp * ez + Ki * e_integral_z

# 【第五步】补偿后角速度更新四元数 → 式 (1.17)
q_dot_w = 0.5 * (-qx * gx - qy * gy - qz * gz)
q_dot_x = 0.5 * ( qw * gx + qy * gz - qz * gy)
q_dot_y = 0.5 * ( qw * gy - qx * gz + qz * gx)
q_dot_z = 0.5 * ( qw * gz + qx * gy - qy * gx)

qw += q_dot_w * dt
qx += q_dot_x * dt
qy += q_dot_y * dt
qz += q_dot_z * dt

# 【第六步】归一化 → 式 (1.26)
norm = np.sqrt(qw**2 + qx**2 + qy**2 + qz**2)
return [qw / norm, qx / norm, qy / norm, qz / norm]